// /****************************** (C) COPYRIGHT 2016 Rsestudio ***********************
// * File Name          : 
// * Author             : Rsestudio
// * Version            : V1.0.0
// * Date               : 01-MAR-2024
// * Description        : 
// *****************************************************************************************/
// /* Includes ----------------------------------------------------------------------------*/
// #include <rtthread.h>
// #include "stm32l4xx.h"
// #include "Bsp.h"
// #include "iProfile_main.h"
// #include "vl53l0x_api.h"
// #include "VL53L0.h"
// /* Private define ----------------------------------------------------------------------*/
// /* Private macro ----------------------------------------------------------------------*/
// /* Private typedef ---------------------------------------------------------------------*/
// /* Extern variables --------------------------------------------------------------------*/
// /* Private variables --------------------------------------------------------------------*/
// /* Private functions -------------------------------------------------------------------*/

// /*****************************************************************************************
// * Function Name  : 
// * Description    : 
// * Input          : None
// * Output         : None
// * Return         : None
// *****************************************************************************************/
// ALIGN(4)
// VL53L0X_Dev_t vl53_dev_1;
// ALIGN(4)
// VL53L0X_Dev_t vl53_dev_2;

// uint32_t refSpadCount = 5;
// uint8_t  isApertureSpads = 0;

// uint8_t  VhvSettings = 26;
// uint8_t  PhaseCal = 1;

// int32_t  OffsetMicroMeter = 25000;
// uint32_t XTalkCompensation = 40;

// int32_t VL53L0_init(void)
// {
//     FixPoint1616_t Temp_value;
     
//     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
     
//     (void)Temp_value;
    
//     vl53_dev_1.I2cDevAddr = 0x52;
//     vl53_dev_1.comms_type = 1;
//     vl53_dev_1.comms_speed_khz = 400;
    
//     Status = VL53L0X_DataInit(&vl53_dev_1);
//     if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
    
//     Status = VL53L0X_StaticInit(&vl53_dev_1);
//     if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
    
// //    Status=VL53L0X_PerformRefSpadManagement(&vl53_dev_1, &refSpadCount, &isApertureSpads); 
// //    if(Status!=VL53L0X_ERROR_NONE) 
// //    { 
// //        return Status; 
// //    }
// //    
// //    Status=VL53L0X_PerformRefCalibration(&vl53_dev_1, &VhvSettings, &PhaseCal);
// //    if(Status!=VL53L0X_ERROR_NONE) 
// //    { 
// //        return Status; 
// //    }
// //    
// //    
// //	Status=VL53L0X_PerformOffsetCalibration(&vl53_dev_1,200<<16,&OffsetMicroMeter);
// //	if(Status!=VL53L0X_ERROR_NONE) 
// //    { 
// //        return Status; 
// //    }

// //	Status=VL53L0X_PerformXTalkCalibration(&vl53_dev_1,200<<16,&XTalkCompensation); 
// //	if(Status!=VL53L0X_ERROR_NONE) 
// //    { 
// //        return Status; 
// //    }
    
//     //----------------------------------------------------------------------------------------------
    
//     Status = VL53L0X_SetReferenceSpads(&vl53_dev_1,refSpadCount,isApertureSpads); 
// 	if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
	
// 	Status=VL53L0X_SetRefCalibration(&vl53_dev_1,VhvSettings,PhaseCal);       
// 	if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
	
// 	Status=VL53L0X_SetOffsetCalibrationDataMicroMeter(&vl53_dev_1,OffsetMicroMeter);
// 	if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
// //	
// 	Status=VL53L0X_SetXTalkCompensationRateMegaCps(&vl53_dev_1,XTalkCompensation);  
// 	if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
// //	
// 	Status = VL53L0X_SetXTalkCompensationEnable(&vl53_dev_1,1);
// 	if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
	
//     //-------------------------------------------------------------------------------------------------------------------
 
// //    Status = VL53L0X_GetLimitCheckCurrent(&vl53_dev,VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, &Temp_value);
// //    if(Status!=VL53L0X_ERROR_NONE) 
// //    { 
// //        return Status; 
// //    }
//     Status = VL53L0X_SetLimitCheckEnable(&vl53_dev_1,VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,1);
//     if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
    
    
//     Status = VL53L0X_SetLimitCheckValue(&vl53_dev_1,VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,(FixPoint1616_t)(0.9*65536));
// 	if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
 
//     Status = VL53L0X_SetLimitCheckValue(&vl53_dev_1,VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,(FixPoint1616_t)(18*65536)); 
//     if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
 
//     Status =VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&vl53_dev_1,200000);
//     if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
 
//     Status = VL53L0X_SetVcselPulsePeriod(&vl53_dev_1,VL53L0X_VCSEL_PERIOD_PRE_RANGE, 18); 
//     if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
 
//     Status = VL53L0X_SetVcselPulsePeriod(&vl53_dev_1,VL53L0X_VCSEL_PERIOD_FINAL_RANGE, 14); 
//     if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
    
//     //---------------------------------------------------------------------------------------------------------------------------------------
    
//     Status = VL53L0X_SetDeviceMode(&vl53_dev_1,VL53L0X_DEVICEMODE_SINGLE_RANGING);
// 	if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
    
// //    Status = VL53L0X_SetDeviceMode(&vl53_dev_1,VL53L0X_DEVICEMODE_CONTINUOUS_RANGING);
// //	if(Status!=VL53L0X_ERROR_NONE) 
// //    { 
// //        return Status; 
// //    }    
	
// //	Status = VL53L0X_StartMeasurement(&vl53_dev_1);
// //	if(Status!=VL53L0X_ERROR_NONE) 
// //    { 
// //        return Status; 
// //    }
    
//     //------------------------------------------------------------------------------------------------------------------------------------------------
    
//     vl53_dev_2.I2cDevAddr = 0x54;
//     vl53_dev_2.comms_type = 1;
//     vl53_dev_2.comms_speed_khz = 400;
    
//     Status = VL53L0X_DataInit(&vl53_dev_2);
//     if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
    
//     Status = VL53L0X_StaticInit(&vl53_dev_2);
//     if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
    
// //    Status=VL53L0X_PerformRefSpadManagement(&vl53_dev_2, &refSpadCount, &isApertureSpads); 
// //    if(Status!=VL53L0X_ERROR_NONE) 
// //    { 
// //        return Status; 
// //    }
// //    
// //    Status=VL53L0X_PerformRefCalibration(&vl53_dev_2, &VhvSettings, &PhaseCal);
// //    if(Status!=VL53L0X_ERROR_NONE) 
// //    { 
// //        return Status; 
// //    }
// //    
// //    
// //	Status=VL53L0X_PerformOffsetCalibration(&vl53_dev_2,200<<16,&OffsetMicroMeter);
// //	if(Status!=VL53L0X_ERROR_NONE) 
// //    { 
// //        return Status; 
// //    }

// //	Status=VL53L0X_PerformXTalkCalibration(&vl53_dev_2,200<<16,&XTalkCompensation); 
// //	if(Status!=VL53L0X_ERROR_NONE) 
// //    { 
// //        return Status; 
// //    }
    
//     //----------------------------------------------------------------------------------------------
    
//     Status = VL53L0X_SetReferenceSpads(&vl53_dev_2,refSpadCount,isApertureSpads); 
// 	if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
	
// 	Status=VL53L0X_SetRefCalibration(&vl53_dev_2,VhvSettings,PhaseCal);       
// 	if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
// //	
// 	Status=VL53L0X_SetOffsetCalibrationDataMicroMeter(&vl53_dev_2,OffsetMicroMeter);
// 	if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
// //	
// 	Status=VL53L0X_SetXTalkCompensationRateMegaCps(&vl53_dev_2,XTalkCompensation);  
// 	if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
	
// 	Status = VL53L0X_SetXTalkCompensationEnable(&vl53_dev_2,1);
// 	if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
	
//     //-------------------------------------------------------------------------------------------------------------------
 
// //    Status = VL53L0X_GetLimitCheckCurrent(&vl53_dev,VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, &Temp_value);
// //    if(Status!=VL53L0X_ERROR_NONE) 
// //    { 
// //        return Status; 
// //    }
     
//     Status = VL53L0X_SetLimitCheckValue(&vl53_dev_2,VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,(FixPoint1616_t)(0.9*65536));
// 	if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
 
//     Status = VL53L0X_SetLimitCheckValue(&vl53_dev_2,VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,(FixPoint1616_t)(18*65536)); 
//     if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
 
//     Status =VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&vl53_dev_2,200000);
//     if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
 
//     Status = VL53L0X_SetVcselPulsePeriod(&vl53_dev_2,VL53L0X_VCSEL_PERIOD_PRE_RANGE, 18); 
//     if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
 
//     Status = VL53L0X_SetVcselPulsePeriod(&vl53_dev_2,VL53L0X_VCSEL_PERIOD_FINAL_RANGE, 14); 
//     if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
    
//     //---------------------------------------------------------------------------------------------------------------------------------------
    
//     Status = VL53L0X_SetDeviceMode(&vl53_dev_2,VL53L0X_DEVICEMODE_SINGLE_RANGING);
// 	if(Status!=VL53L0X_ERROR_NONE) 
//     { 
//         return Status; 
//     }
    
// //    Status = VL53L0X_SetDeviceMode(&vl53_dev_2,VL53L0X_DEVICEMODE_CONTINUOUS_RANGING);
// //	if(Status!=VL53L0X_ERROR_NONE) 
// //    { 
// //        return Status; 
// //    }
// //	
// //	Status = VL53L0X_StartMeasurement(&vl53_dev_2);
// //	if(Status!=VL53L0X_ERROR_NONE) 
// //    { 
// //        return Status; 
// //    }
    
//     return 0;
// }

// ALIGN(4)
// VL53L0X_RangingMeasurementData_t meas_data_1;

// ALIGN(4)
// VL53L0X_RangingMeasurementData_t meas_data_2;

// uint16_t ls_timer;
// uint16_t ls_routine;
// int32_t VL53L0X_polling(void)
// {
//     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
    
//     if(iProfile.sys_status.BatVoltage < 3000)
//     {
//         return 0;
//     }
    
//     switch(ls_routine)
//     {
//         case 0:
//         {
//             Status = VL53L0X_StartMeasurement(&vl53_dev_1);
//             if(Status!=VL53L0X_ERROR_NONE) 
//             { 
//                 return Status; 
//             }
//             ls_timer = 0;
//             ls_routine = 1;
			
// 			iProfile.sys_status.v_data_buf[V_ID_SPEED] =	(iProfile.speed.s1_value<<16)|(iProfile.speed.s2_value);
//         }
//         break;
        
//         case 1:
//         {
//             if(ls_timer++>8)
//             {
//                 ls_timer = 0;
//                 ls_routine = 2;                
//             }
//         }
//         break;
        
//         case 2:
//         {
//             VL53L0X_GetRangingMeasurementData(&vl53_dev_1,&meas_data_1);
//             ls_timer = 0;
//             ls_routine = 3;
// 			iProfile.speed.s1_value = meas_data_1.RangeMilliMeter;
//         }break;
        
//         case 3:
//         {
//             Status = VL53L0X_StartMeasurement(&vl53_dev_2);
//             if(Status!=VL53L0X_ERROR_NONE) 
//             { 
//                 return Status; 
//             }
//             ls_timer = 0;
//             ls_routine = 4;
//         }
        
//         case 4:
//         {
//              if(ls_timer++>8)
//             {
//                 ls_timer = 0;
//                 ls_routine = 5;                
//             }
//         }break;
        
//         case 5:
//         {
//             VL53L0X_GetRangingMeasurementData(&vl53_dev_2,&meas_data_2);
//             ls_timer = 0;
//             ls_routine = 0;
// 			iProfile.speed.s2_value = meas_data_2.RangeMilliMeter;
//         }break;
        
//         default:
//         {
//             ls_timer = 0;
//             ls_routine = 0;
//         }break;
//     }
// //    VL53L0X_GetRangingMeasurementData(&vl53_dev_1,&meas_data_1);
// //    VL53L0X_GetRangingMeasurementData(&vl53_dev_2,&meas_data_2);
	
// 	return 0;

// }

// /**********************************(C)COPYRIGHT 2016 Rsestudio ************************/
// /*****************************************END OF FILE**********************************/

